#pragma config(Sensor, in1,    sensorLeft,     sensorLineFollower)
#pragma config(Sensor, in2,    sensorMiddle,   sensorLineFollower)
#pragma config(Sensor, in3,    sensorRight,    sensorLineFollower)
#pragma config(Sensor, in4,    clawAngle,      sensorPotentiometer)
#pragma config(Sensor, in5,    armAngle,       sensorPotentiometer)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  sonarInput,     sensorSONAR_inch)
#pragma config(Motor,  port1,           rightMotor,    tmotorVex393, openLoop)
#pragma config(Motor,  port2,           rightMotorFront, tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port5,           arm,           tmotorVex393, openLoop)
#pragma config(Motor,  port6,           claw,          tmotorVex393, openLoop)
#pragma config(Motor,  port9,           leftMotorFront, tmotorVex393, openLoop)
#pragma config(Motor,  port10,          leftMotor,     tmotorVex393, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int armAngleInt = 0;
int clawAngleInt = 0;
int encoderRightInt = 0;
int encoderLeftInt = 0;
int placeholder = 0;
int sensorLeftInt = 0;
int sensorRightInt = 0;
int sensorMiddleInt = 0;


//+++++++++++++++++++++++++++++++++++++++ Pause +++++++++++++++++++++++++++++++++++++++++

void pauseReset(int seconds) {
	motor[leftMotor] = motor[rightMotor] = motor[leftMotorFront] = motor[rightMotorFront] = 0;

	wait1Msec(seconds * 1000);

	sensorLeftInt = 0;
	sensorRightInt = 0;
	sensorMiddleInt = 0;

	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

	encoderLeftInt = 0;
	encoderRightInt = 0;

	placeholder = 0;
}

void assist(){

	if(abs(vexRT[Ch3]) >= 10 || abs(vexRT[Ch2]) >= 10)
	{
		motor[leftMotor] = motor[leftMotorFront] = vexRT[Ch3];
		motor[rightMotor] = motor[rightMotorFront] = vexRT[Ch2];
		wait1Msec(1);
	}

}

//++++++++++++++++++++++++++++++++++++++++++ U-Turn Left +++++++++++++++++++++++++++++++++++++++++++

void uturnLeft() {

	motor[leftMotor] = 50;
	motor[rightMotor] = 50;
	motor[leftMotorFront] = 50;
	motor[rightMotorFront] = 50;

	wait1Msec(890);

	pauseReset(1);

	while(placeholder == 0){

		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		motor[leftMotor] = -50;
		motor[rightMotor] = 50;
		motor[leftMotorFront] = -50;
		motor[rightMotorFront] = 50;

		if(sensorLeftInt > 2000){
			wait1Msec(100);
			motor[leftMotor] = 70;
			motor[rightMotor] = -70;
			motor[leftMotorFront] = 70;
			motor[rightMotorFront] = -70;
			wait1Msec(45);
			placeholder++;
		}
	}

}

//++++++++++++++++++++++++++++++++++++++++++ U-Turn Right +++++++++++++++++++++++++++++++++++++++++++

void uturnRight() {

	motor[leftMotor] = 50;
	motor[rightMotor] = 50;
	motor[leftMotorFront] = 50;
	motor[rightMotorFront] = 50;

	wait1Msec(890);

	pauseReset(1);

	while(placeholder == 0){

		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		motor[leftMotor] = 50;
		motor[rightMotor] = -50;
		motor[leftMotorFront] = 50;
		motor[rightMotorFront] = -50;

		if(sensorRightInt > 2000){
			wait1Msec(100);
			motor[leftMotor] = -70;
			motor[rightMotor] = 70;
			motor[leftMotorFront] = -70;
			motor[rightMotorFront] = 70;
			wait1Msec(45);
			placeholder++;
		}

		assist();

	}

}

//++++++++++++++++++++++ Travel in a straight line until intersection left ++++++++++++++++++++

void lineUIL() {

	while(placeholder == 0){
		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		if(sensorMiddleInt > 2000){
			motor[leftMotor] = 50;
			motor[rightMotor] = 50;
			motor[leftMotorFront] = 50;
			motor[rightMotorFront] = 50;
		}

		if(sensorRightInt > 2000){
			motor[leftMotor] = 70;
			motor[rightMotor] = 35;
			motor[leftMotorFront] = 70;
			motor[rightMotorFront] = 35;
		}

		if(sensorLeftInt > 2000){
			motor[leftMotor] = 35;
			motor[rightMotor] = 70;
			motor[leftMotorFront] = 35;
			motor[rightMotorFront] = 70;
		}

		if(sensorLeftInt > 2000 && sensorMiddleInt > 2000){
			wait1Msec(83);
			motor[leftMotor] = motor[rightMotor] = motor[leftMotorFront] = motor[rightMotorFront] = 0;
			placeholder++;
		}

		assist();

	}

}

//++++++++++++++++++++++ Travel in a straight line until intersection right ++++++++++++++++++++

void lineUIR() {

	while(placeholder == 0){
		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		if(sensorMiddleInt > 2000){
			motor[leftMotor] = 50;
			motor[rightMotor] = 50;
			motor[leftMotorFront] = 50;
			motor[rightMotorFront] = 50;
		}

		if(sensorRightInt > 2000){
			motor[leftMotor] = 70;
			motor[rightMotor] = 35;
			motor[leftMotorFront] = 70;
			motor[rightMotorFront] = 35;
		}

		if(sensorLeftInt > 2000){
			motor[leftMotor] = 35;
			motor[rightMotor] = 70;
			motor[leftMotorFront] = 35;
			motor[rightMotorFront] = 70;
		}

		if(sensorRightInt > 2000 && sensorMiddleInt > 2000){
			wait1Msec(90);
			motor[leftMotor] = motor[leftMotorFront] = 0;
			wait1Msec(30);
			motor[rightMotor] = motor[rightMotorFront] = 0;
			placeholder++;
		}

		assist();

	}

}

//+++++++++++++++++++++++++++++++++++++++ Turn right +++++++++++++++++++++++++++++++++++++++++

void rightTurn() {

	while(placeholder == 0){

		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		motor[leftMotor] = 50;
		motor[rightMotor] = -50;
		motor[leftMotorFront] = 50;
		motor[rightMotorFront] = -50;

		if(sensorRightInt > 2000){
			wait1Msec(150);
			motor[leftMotor] = -50;
			motor[rightMotor] = 50;
			motor[leftMotorFront] = -50;
			motor[rightMotorFront] = 50;
			wait1Msec(65);
			placeholder++;
		}

		assist();

	}

}

//+++++++++++++++++++++++++++++++++++++++ Turn left +++++++++++++++++++++++++++++++++++++++++

void leftTurn() {

	while(placeholder == 0){

		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		motor[leftMotor] = -50;
		motor[rightMotor] = 50;
		motor[leftMotorFront] = -50;
		motor[rightMotorFront] = 50;

		if(sensorLeftInt > 2000){
			wait1Msec(150);
			motor[leftMotor] = 50;
			motor[rightMotor] = -50;
			motor[leftMotorFront] = 50;
			motor[rightMotorFront] = -50;
			wait1Msec(65);
			placeholder++;
		}

		assist();

	}

}

//+++++++++++++++++++++++++++++++++++++++ Exit House +++++++++++++++++++++++++++++++++++++++++

void exit(int intersections) {

	while(placeholder < intersections){
		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		if(sensorMiddleInt > 2000){
			motor[leftMotor] = 60;
			motor[rightMotor] = 60;
			motor[leftMotorFront] = 60;
			motor[rightMotorFront] = 60;
		}

		if(sensorRightInt > 2000){
			motor[leftMotor] = 80;
			motor[rightMotor] = 45;
			motor[leftMotorFront] = 80;
			motor[rightMotorFront] = 45;
		}

		if(sensorLeftInt > 2000){
			motor[leftMotor] = 45;
			motor[rightMotor] = 80;
			motor[leftMotorFront] = 45;
			motor[rightMotorFront] = 80;
		}

		if(sensorLeftInt > 2000 && sensorRightInt > 2000 && sensorMiddleInt > 2000) {
			//motor[leftMotor] = motor[rightMotor] = motor[leftMotorFront] = motor[rightMotorFront] = 0;

			placeholder++;

		}

		assist();

	}

}

//+++++++++++++++++++++++++++++++++++++++++++ Arm Routine +++++++++++++++++++++++++++++++++++++++++++++

void armRoutine() {
	armAngleInt = SensorValue[armAngle];

	clawAngleInt = SensorValue[clawAngle];

	while(armAngleInt > 375) {
		motor[arm] = 50;
		armAngleInt = SensorValue[armAngle];
	}

	wait10Msec(100);

	while(clawAngleInt > 1700) {
		clawAngleInt = SensorValue[clawAngle];
		motor[claw] = 100;
	}

	wait10Msec(100);

	while(clawAngleInt < 3900) {
		clawAngleInt = SensorValue[clawAngle];
		motor[claw] = -127;

	}

	wait10Msec(100);

	while(armAngleInt < 1575) {
		motor[arm] = -120;
		armAngleInt = SensorValue[armAngle];
	}
}


//+++++++++++++++++++++++++++++++++++++++++++++| Route 1 |+++++++++++++++++++++++++++++++++++++++++++++

void route1() {

	lineUIR();

	pauseReset(1);

	rightTurn();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	rightTurn();

	pauseReset(1);

	armRoutine();

	pauseReset(1);

	uturnRight();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	exit(1);

	pauseReset(1);

}

//+++++++++++++++++++++++++++++++++++++++++++++| Route 2 |+++++++++++++++++++++++++++++++++++++++++++++

void route2() {

	lineUIR();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	rightTurn();

	pauseReset(1);

	armRoutine();

	pauseReset(1);

	motor[leftMotor] = 50;
	motor[rightMotor] = 50;
	motor[leftMotorFront] = 50;
	motor[rightMotorFront] = 50;

	wait1Msec(650);

	pauseReset(1);

	while(placeholder == 0){

		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		motor[leftMotor] = 50;
		motor[rightMotor] = -50;
		motor[leftMotorFront] = 50;
		motor[rightMotorFront] = -50;

		if(sensorRightInt > 2000){
			wait1Msec(100);
			motor[leftMotor] = -70;
			motor[rightMotor] = 70;
			motor[leftMotorFront] = -70;
			motor[rightMotorFront] = 70;
			wait1Msec(45);
			placeholder++;
		}

		assist();

	}

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	rightTurn();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	rightTurn();

	pauseReset(1);

	exit(1);

	pauseReset(1);

}

//+++++++++++++++++++++++++++++++++++++++++++++| Route 3 |+++++++++++++++++++++++++++++++++++++++++++++

void route3() {

	lineUIR();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	rightTurn();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	rightTurn();

	pauseReset(1);

	armRoutine();

	pauseReset(1);

	uturnRight();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	rightTurn();

	exit(1);

	pauseReset(1);

}

//+++++++++++++++++++++++++++++++++++++++++++++| Route 4 |+++++++++++++++++++++++++++++++++++++++++++++

void route4() {

	lineUIR();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	rightTurn();

	pauseReset(1);

	motor[leftMotor] = 50;
	motor[rightMotor] = 50;
	motor[leftMotorFront] = 50;
	motor[rightMotorFront] = 50;

	wait1Msec(350);

	pauseReset(1);

	armRoutine();

	pauseReset(1);

	motor[leftMotor] = 50;
	motor[rightMotor] = 50;
	motor[leftMotorFront] = 50;
	motor[rightMotorFront] = 50;

	wait1Msec(650);

	pauseReset(1);

	while(placeholder == 0){

		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		motor[leftMotor] = 50;
		motor[rightMotor] = -50;
		motor[leftMotorFront] = 50;
		motor[rightMotorFront] = -50;

		if(sensorRightInt > 2000){
			wait1Msec(100);
			motor[leftMotor] = -70;
			motor[rightMotor] = 70;
			motor[leftMotorFront] = -70;
			motor[rightMotorFront] = 70;
			wait1Msec(45);
			placeholder++;
		}

		assist();

	}

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	exit(2);

	pauseReset(1);

}

//+++++++++++++++++++++++++++++++++++++++++++++| Route 5 |+++++++++++++++++++++++++++++++++++++++++++++

void route5() {

	lineUIR();

	pauseReset(1);

	lineUIR();

	pauseReset(1);

	rightTurn();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	armRoutine();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	leftTurn();

	pauseReset(1);

	lineUIL();

	pauseReset(1);

	exit(2);

	pauseReset(1);

}

//+++++++++++++++++++++++++++++++++++++++++++++| Route 6 |+++++++++++++++++++++++++++++++++++++++++++++

void route6() {

	while(1 == 1){
		motor[leftMotor] = motor[leftMotorFront] = vexRT[Ch3];
		motor[rightMotor] = motor[rightMotorFront] = vexRT[Ch2];
		    if(vexRT[Btn5U] == 1) {
    	motor[claw] = 127;
    }
    if(vexRT[Btn6U] == 1) {
    	motor[claw] = -127;
    }
    if(vexRT[Btn5U] == 0 && vexRT[Btn6U] == 0) {
    	motor[claw] = 0;
    }

    if(vexRT[Btn5D] == 1) {
    	motor[arm] = 127;
    }
    if(vexRT[Btn6D] == 1) {
    	motor[arm] = -127;
    }
    if(vexRT[Btn5D] == 0 && vexRT[Btn6D] == 0) {
    	motor[arm] = 0;
    }


  }

}

//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |++++++++++++++++++++++++++++++++++++++++++++++++++

task main ()
{

	int buttonPressed = 0;

	while(buttonPressed == 0) {

		if(vexRT[Btn7D] == 1) {
			route1();
			buttonPressed++;
		}

		if(vexRT[Btn7L] == 1) {
			route2();
			buttonPressed++;
		}

		if(vexRT[Btn7U] == 1) {
			route3();
			buttonPressed++;
		}

		if(vexRT[Btn7R] == 1) {
			route4();
			buttonPressed++;
		}

		if(vexRT[Btn8D] == 1) {
			route5();
			buttonPressed++;
		}

		if(vexRT[Btn8L] == 1) {
			route6();
			buttonPressed++;
		}

	}

}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
